/******************************************************************************
* Copyright (c) 2015-2025 jiangxiaogang<kerndev@foxmail.com>
*
* This file is part of KLite distribution.
*
* KLite is free software, you can redistribute it and/or modify it under
* the MIT Licence.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
******************************************************************************/
#include "internal.h"
#include "kernel.h"
#include "list.h"

struct tcb             *sched_tcb_now;
struct tcb             *sched_tcb_next;
static struct tcb_list  m_list_ready[8];
static struct tcb_list  m_list_sleep[1];
static struct tcb_list  m_list_pend[1];
static uint32_t         m_idle_elapse;
static uint32_t         m_idle_timeout;
static uint32_t         m_prio_bitmap;
static const int8_t     m_prio_table[256]=
{
	-1,0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3,
	4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4,
	5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
	5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
	6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
	6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
	6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
	6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
};

#define m_prio_highest (m_prio_table[m_prio_bitmap & 0xff])

static void list_insert_by_priority(struct tcb_list *list, struct tcb_node *node)
{
	struct tcb_node *find;
	uint32_t prio = node->tcb->prio;
	for(find = list->tail; find != NULL; find = find->prev)
	{
		if(find->tcb->prio >= prio)
		{
			break;
		}
	}
	list_insert_after(list, find, node);
}

void sched_remove(struct tcb *tcb)
{
	if(tcb->list_wait)
	{
		list_remove(tcb->list_wait, &tcb->node_wait);
		tcb->list_wait = NULL;
	}
	if(tcb->list_sched)
	{
		list_remove(tcb->list_sched, &tcb->node_sched);
		if(tcb->list_sched == &m_list_ready[tcb->prio])
		{
			if(tcb->list_sched->head == NULL)
			{
				m_prio_bitmap &= ~(1 << tcb->prio);
			}
		}
		tcb->list_sched = NULL;
	}
}

void sched_reset(struct tcb *tcb, uint32_t prio)
{
	if(tcb->list_wait)
	{
		list_remove(tcb->list_wait, &tcb->node_wait);
		list_insert_by_priority(tcb->list_wait, &tcb->node_wait);
	}
	if(tcb->list_sched)
	{
		if(tcb->list_sched == &m_list_ready[tcb->prio])
		{
			/* Remove from old list */
			list_remove(tcb->list_sched, &tcb->node_sched);
			if(tcb->list_sched->head == NULL)
			{
				m_prio_bitmap &= ~(1 << tcb->prio);
			}
			/* Append to new list */
			tcb->list_sched = &m_list_ready[prio];
			list_append(tcb->list_sched, &tcb->node_sched);
			m_prio_bitmap |= (1 << prio);
		}
	}
}

void sched_ready(struct tcb *tcb)
{
	tcb->list_sched = &m_list_ready[tcb->prio];
	list_append(tcb->list_sched, &tcb->node_sched);
	m_prio_bitmap |= (1 << tcb->prio);
}

static void sched_wake_up(struct tcb *tcb)
{
	if(tcb->list_wait)
	{
		list_remove(tcb->list_wait, &tcb->node_wait);
		tcb->list_wait = NULL;
	}
	if(tcb->list_sched)
	{
		list_remove(tcb->list_sched, &tcb->node_sched);
	}
	sched_ready(tcb);
}

static uint32_t sched_sleep_timeout(uint32_t elapse)
{
	struct tcb *tcb;
	struct tcb_node *node;
	struct tcb_node *next;
	uint32_t timeout = UINT32_MAX;
	for(node = m_list_sleep->head; node != NULL; node = next)
	{
		next = node->next;
		tcb = node->tcb;
		if(tcb->timeout > elapse)
		{
			tcb->timeout -= elapse;
			if(tcb->timeout < timeout)
			{
				timeout = tcb->timeout;
			}
		}
		else
		{
			tcb->timeout = 0;
			sched_wake_up(tcb);
		}
	}
	return timeout;
}

void sched_sleep(uint32_t timeout)
{
	struct tcb *tcb = sched_tcb_now;
	tcb->timeout = timeout + m_idle_elapse;
	if(tcb->timeout < timeout) /* Fixup tcb->timeout overflow! */
	{
		m_idle_timeout = sched_sleep_timeout(m_idle_elapse);
		m_idle_elapse = 0;
		tcb->timeout = timeout;
	}
	if(tcb->timeout < m_idle_timeout)
	{
		m_idle_timeout = tcb->timeout;
	}
	tcb->list_sched = m_list_sleep;
	list_append(tcb->list_sched, &tcb->node_sched);
}

void sched_wait(struct tcb_list *list)
{
	struct tcb *tcb = sched_tcb_now;
	tcb->list_wait = list;
	list_insert_by_priority(list, &tcb->node_wait);
	tcb->list_sched = m_list_pend;
	list_append(tcb->list_sched, &tcb->node_sched);
}

void sched_timed_wait(struct tcb_list *list, uint32_t timeout)
{
	struct tcb *tcb = sched_tcb_now;
	tcb->list_wait = list;
	list_insert_by_priority(list, &tcb->node_wait);
	sched_sleep(timeout);
}

struct tcb *sched_wake_from(struct tcb_list *list)
{
	struct tcb *tcb;
	if(list->head)
	{
		tcb = list->head->tcb;
		sched_wake_up(tcb);
		return tcb;
	}
	return NULL;
}

void sched_switch(void)
{
	struct tcb_list *list = &m_list_ready[m_prio_highest];
	struct tcb *tcb = list->head->tcb;
	list_remove(tcb->list_sched, &tcb->node_sched);
	if(tcb->list_sched->head == NULL)
	{
		m_prio_bitmap &= ~(1 << tcb->prio);
	}
	tcb->list_sched = NULL;
	sched_tcb_next = tcb;
	cpu_contex_switch();
}

void sched_preempt(void)
{
	if((int)m_prio_highest > (int)sched_tcb_next->prio)
	{
		sched_ready(sched_tcb_next);
		sched_switch();
	}
}

void sched_tick(uint32_t time)
{
	m_idle_elapse += time;
	if(m_idle_elapse >= m_idle_timeout)
	{
		m_idle_timeout = sched_sleep_timeout(m_idle_elapse);
		m_idle_elapse = 0;
	}
	sched_tcb_now->time += time;
	if(sched_tcb_now == sched_tcb_next)
	{
		if((int)m_prio_highest >= (int)sched_tcb_now->prio)
		{
			sched_ready(sched_tcb_now);
			sched_switch();
		}
	}
}

uint32_t sched_idle(void)
{
	if(m_prio_bitmap != 0)
	{
		sched_ready(sched_tcb_now);
		sched_switch();
		return 0;
	}
	return m_idle_timeout;
}

void sched_init(void)
{
	sched_tcb_now = NULL;
	sched_tcb_next = NULL;
	m_idle_timeout = UINT32_MAX;
}
